; RobotTest.asm (ASM)
; This program performs tests of robot hardware.
; Taylor Burdette, Louae Tyoan, James Nagendran, Troy O'Neal, Zack Tufford
; ECE 2031 LO7
; 04/11/13

	ORG     &H000		;Begin program at x000
Init:		JUMP	OutputTest


; mechanism for jumping to an arbitrary test
TestTarget:	DW	0
TestTargetMask:	DW	&H0007
JumpOpCodeMask:	DW	&H1400
; jumps to test in TestTarget
JumpToTest:
	LOAD	TestTarget
	
	AND		TestTargetMask
	
	ADDI	TestTable ; now ACC = addr(target)
	STORE	Temp
	ILOAD	Temp ; ACC = target
	OR		JumpOpCodeMask		; ACC = instruction to jump to target
	STORE	JumpInstr ; hack to jump to a variable place
	
JumpInstr:	DW	0


TestTable:		DW	OutputTest
				DW	SCOMPTest
				DW	BatteryTest
				DW	MotorTest
				DW	AudioTest
				DW	SonarTest	
				DW	0
				DW	0
	

;; Basic I/O
OutputTest:
	;JUMP	SonarTest

	LOAD    Zero
	STORE	FailCode
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	LOAD	AllOnes		; See if LEDs are working
	OUT		LEDS
	LOAD	Eights		; See if Seven segment is working
	OUT		SEVENSEG
	

	CALL	ClearLCD
	LOAD	Zero
	ADDI	ChkIOStr_0
	STORE	StrStart
	CALL	StrToLCD
	
	
	CALL SpinKey
	CALL	ClearLCD
	LOAD	One
	STORE	TestTarget
	JUMP	JumpToTest
	
	;; MORSE CODE TESTING FUNCTIONS
	
WORDSPACE:
	LOAD	Zero
	OUT		AUDIO_CTL
	CALL	SIXWAIT
	RETURN
DAHBLIT:
	LOAD 	One
	OUT 	AUDIO_CTL
	CALL	THREEWAIT
	RETURN
CHARSPACE:
	LOAD	Zero
	OUT		AUDIO_CTL
	CALL	THREEWAIT
	RETURN
DITBLIT:
	LOAD	One
	OUT		AUDIO_CTL
	CALL	ONEWAIT
	RETURN
SYMSPACE:
	LOAD	Zero
	OUT		AUDIO_CTL
	CALL	ONEWAIT
	RETURN
ONEWAIT:
	OUT		TIMER
ONEWAIT_LOOP:
	IN		TIMER
	ADDI	-1
	JNEG	ONEWAIT_LOOP
	RETURN
THREEWAIT:
	OUT 	TIMER
THREEWAIT_LOOP:
	IN		TIMER
	ADDI	-3
	JNEG	THREEWAIT_LOOP
	RETURN
SIXWAIT:
	OUT		TIMER
SIXWAIT_LOOP:
	IN	TIMER
	ADDI	-6
	JNEG	SIXWAIT_LOOP
	RETURN
	
;; the following code runs a series of tests on the
;; core instruction set, as a sanity check.  Before anything else
;; is tested, the CPU should be working.
SCOMPTest:
TestInstructionSet:
	; pre-emptively load failure code
	LOAD	FAIL_INST_SET
	STORE	FailCode
	
	; tests basic load, jump instructions as well as operators
	LOAD	ST_OPERAND_1_ADD_1 ; begin test of ADD
	ADD		ST_OPERAND_2_ADD_1
	SUB		ST_RESULT_ADD_1		; if ADD works, A = 0
	JZERO	tis0
	JUMP	CritFail				; if ADD failed, we'll be here
tis0:


	LOAD	ST_OPERAND_1_ADD_2 ; begin test of ADD
	ADD		ST_OPERAND_2_ADD_2
	SUB		ST_RESULT_ADD_2		; if ADD works, A = 0
	JZERO	tis1
	JUMP	CritFail				; if ADD failed, we'll be here
tis1:
	LOAD	ST_OPERAND_1_SUB_1 ; begin test of SUB
	SUB		ST_OPERAND_2_SUB_1
	SUB		ST_RESULT_SUB_1		; if SUB works, A = 0
	JZERO	tis2
	JUMP	CritFail				; if SUB failed, we'll be here
tis2:

	LOAD	ST_OPERAND_1_SUB_2 ; begin test of SUB 2
	SUB		ST_OPERAND_2_SUB_2
	SUB		ST_RESULT_SUB_2	; if SUB works, A = 0
	JZERO	tis3
	JUMP	CritFail				; if SUB failed, we'll be here
tis3:
	LOAD	ST_OPERAND_1_AND_1 ; begin test of AND
	AND		ST_OPERAND_2_AND_1
	SUB		ST_RESULT_AND_1		; if AND works, A = 0
	JZERO	tis4
	JUMP	CritFail				; if AND failed, we'll be here
			
tis4:
	LOAD	ST_OPERAND_1_AND_2 ; begin test of AND 2
	AND		ST_OPERAND_2_AND_2
	SUB		ST_RESULT_AND_2	; if AND works, A = 0
	JZERO	tis5
	JUMP	CritFail				; if AND failed, we'll be here
tis5:
	LOAD	ST_OPERAND_1_OR_1 ; begin test of OR
	OR		ST_OPERAND_2_OR_1
	SUB		ST_RESULT_OR_1		; if OR works, A = 0
	JZERO	tis6
	JUMP	CritFail				; if OR failed, we'll be here

tis6:

	LOAD	ST_OPERAND_1_OR_2 ; begin test of OR 2
	OR		ST_OPERAND_2_OR_2
	SUB		ST_RESULT_OR_2	; if OR works, A = 0
	JZERO	tis7
	JUMP	CritFail				; if OR failed, we'll be here
tis7:

	LOAD	ST_OPERAND_1_XOR_1 ; begin test of XOR
	XOR		ST_OPERAND_2_XOR_1
	SUB		ST_RESULT_XOR_1		; if XOR works, A = 0
	JZERO	tis8
	JUMP	CritFail				; if XOR failed, we'll be here
tis8:
	
	LOAD	ST_OPERAND_1_XOR_2 ; begin test of XOR 2
	XOR		ST_OPERAND_2_XOR_2
	SUB		ST_RESULT_XOR_2	; if XOR works, A = 0
	JZERO	tis9
	JUMP	CritFail				; if XOR failed, we'll be here
tis9:
	
	LOAD	ST_OPERAND_1_SHIFT_RIGHT ; begin test of SHIFT
	SHIFT	-1
	SUB		ST_RESULT_SHIFT_RIGHT	; if SHIFT works, A = 0
	JZERO	tis10
	JUMP	CritFail				; if SHIFT failed, we'll be here
tis10:

	LOAD	ST_OPERAND_1_SHIFT_LEFT ; begin test of SHIFT 2
	SHIFT	1
	SUB		ST_RESULT_SHIFT_LEFT	; if SHIFT works, A = 0
	JZERO	tis11
	JUMP	CritFail				; if SHIFT failed, we'll be here
tis11:

	LOAD	ST_OPERAND_1_ADDI_1 ; begin test of ADDI
	ADDI	&H0151
	SUB		ST_RESULT_ADDI_1	; if ADDI works, A = 0
	JZERO	tis12
	JUMP	CritFail				; if ADDI failed, we'll be here
tis12:
	LOAD	ST_OPERAND_1_ADDI_2 ; begin test of ADDI 2
	ADDI	-131
	SUB		ST_RESULT_ADDI_2	; if ADDI works, A = 0
	JZERO	tis13
	JUMP	CritFail				; if ADDI failed, we'll be here
tis13:
	;; here we jump to wherever we need to go next

	CALL	ShowLine2
	LOAD	Zero
	ADDI	SCOMPPassStr_0
	STORE	StrStart
	CALL	StrToLCD
	CALL	SpinKey
	CALL	ClearLCD
	
	
	; battery test start
BatteryTest:	
	;---------------------------------------------------------------
	; store appropriate fail codes
		LOAD	Batteryw		;Initialize
	OUT		I2C_CMD		;
	LOAD	Zero
	OUT		I2C_DATA
BUSY1:
	IN		I2C_BUSY	;IF I2C IS PRSOCESSING
	JZERO	NB1
	JUMP	BUSY1
	;IN		I2C_DATA
NB1: CALL	WAIT	
	LOAD	Batteryr		;First Reading
	OUT		I2C_CMD		;
	LOAD	Zero
	OUT		I2C_DATA
BUSY2:
	IN		I2C_BUSY	;IF I2C IS PRSOCESSING
	JZERO	NB2
	JUMP	BUSY2
NB2: CALL	WAIT	
	IN		I2C_DATA
	LOAD	Batteryr		;First Reading
	OUT		I2C_CMD		;
	LOAD	Zero
	OUT		I2C_DATA
BUSY3:
	IN		I2C_BUSY	;IF I2C IS PRSOCESSING
	JZERO	NB3
	JUMP	BUSY3
NB3: CALL	WAIT	
	IN		I2C_DATA
	OUT		SEVENSEG	;Hex battery level


	JZERO	FAILADC		;Fail ADC
	JNEG	FAILADC		;No Battery
	SUB		BadBatt
	JZERO	FAILBATT		;Low Battery
	JNEG	FAILBATT	;Low Battery
	JUMP	BattOK
	
WAIT:
	OUT 	TIMER
W2:	IN 		TIMER
	SUB 	Twen
	JNEG	W2
	RETURN
	
FAILADC:
	LOAD	FAIL_ADC
	STORE	FailCode
	CALL	Fail
	JUMP	BattTestDone
	
FAILBATT:
	LOAD	FAIL_LOWBATTERY
	STORE	FailCode
	CALL	Fail
	JUMP	BattTestDone

BattOK:	
	LOAD	Zero
	ADDI	BattDoneStr_0	; show battery OK message
	STORE	StrStart
	CALL	StrToLCD
	CALL	SpinKey
	CALL	ClearLCD
	JUMP	BattTestDone
	
Batteryr:  DW &H0190	;W1R1 Address 90
Batteryw:  DW &H1090
BadBatt:  DW &H6C
Twen:	DW 5	
	
BattTestDone:	
	
	JUMP	MotorTest
	


	
	; motor test start
	
MotorTest:
	
	;OUTPUT to LCD "Toggle Switch 17, press Key1"
	;INSERT A KEY1 Spincall switch
	
	LOAD	Zero
	ADDI	PreMotorStr_0
	STORE	StrStart
	CALL	StrToLCD
	
	;Remove these waits when Keytoggle has been implemented
	CALL	SpinKey
	CALL	ClearLCD
	
	LOAD	Zero
	ADDI	MotorWaitStr_0
	STORE	StrStart
	CALL	StrToLCD

	LOAD	Zero
	OUT		TIMER
	; reset fail code
	LOAD	Zero
	OUT		FailCode
	
	LOAD    Zero		;DISABLE EVERYTHING
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	
	CALL	Wait1
	
	

	;INSERT A KEY1 Spincall switch
	
	
	LOAD	Zero
	STORE	FailCode
	
	LOAD	Zero
	ADDI	6
	STORE	RepeatSix
	
	LOAD	Zero
	ADDI	Forward1
	STORE	SpeedPTR

	LOAD	Zero
	ADDI	RangeF1L
	STORE	LowPTR

	LOAD	Zero
	ADDI	RangeF1H
	STORE	HighPTR
	

	
MOTOR:
	Load	Zero
	OUT		TIMER
SPIN: 				;spin forward slow
	ILOAD 	SpeedPTR		;GET THE WHEELS SPINNING FORWARD
	OUT		LVELCMD
	OUT		RVELCMD
	IN      LVEL        ; Read velocity of wheels
	STORE   Temp
	IN      RVEL
	ADD     Temp
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	IN		TIMER
	ADDI	-20
	JNEG	SPIN		;REPEAT FOR 2 SECONDS
	
	IN		LVEL		;Test Left Motor and encoder
	STORE	Speed
	JZERO	LSTOP	;No speed
LSTOPB:	ILOAD	LowPTR
	SUB		Speed
	JPOS	LFAIL	;TOO SLOW
	ILOAD	HighPTR	
	SUB		SPEED
	JNEG	LFAIL	;TOO FAST
LFAILB:
		
	IN		RVEL		;Test Right Motor and encoder
	STORE	Speed
	JZERO	RSTOP	;No speed
RSTOPB:	ILOAD	LowPTR
	SUB		Speed
	JPOS	RFAIL	;TOO SLOW
	ILOAD	HighPTR	
	SUB		SPEED
	JNEG	RFAIL	;TOO FAST
RFAILB:	

LOAD	SpeedPTR
ADDI	1
STORE	SpeedPTR

LOAD	LowPTR
ADDI	1
STORE	LowPTR

LOAD	HighPTR
ADDI	1
STORE	HighPTR
	
LOAD	RepeatSix
ADDI	-1
STORE	RepeatSix
JPOS	MOTOR
	
	
	
;Clear again
	LOAD    Zero
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	LOAD	FAILCODE
	JZERO	MotorPass
	CALL	Fail
	JUMP	AudioTest
	
	

	

Forward1:  DW 70
Forward2:  DW 100
Forward3:  DW 120
Reverse1:  DW -70
Reverse2:  DW -100
Reverse3:  DW -120

RangeF1L:  DW  &H05
RangeF2L:  DW  &H13
RangeF3L:  DW  &H20
RangeR1L:  DW  &HFFD9
RangeR2L:  DW  &HFFCA
RangeR3L:  DW  &HFFBD

RangeF1H:  DW  &H27
RangeF2H:  DW  &H36
RangeF3H:  DW  &H43
RangeR1H:  DW  &HFFFB
RangeR2H:  DW  &HFFED
RangeR3H:  DW  &HFFE0


SpeedPTR:	DW	0
LowPTR:		DW	0
HighPTR:	DW	0

	

	
	
LFAIL:
	;Left Wheel passed threshold
	LOAD	FailCode
	OR		FAIL_LEFTMOTORVELTHRESH
	STORE	FailCode
	JUMP	LFAILB
	
RFAIL:
	;Right Wheel passed threshold
	LOAD	FailCode
	OR		FAIL_RIGHTMOTORVELTHRESH
	STORE	FailCode
	JUMP	RFAILB
	
LSTOP:
	;Left Doesn't move
	LOAD	FailCode
	OR		FAIL_LEFTMOTORNOSTART
	STORE	FailCode
	JUMP	LSTOPB
	
RSTOP:
	;Right doen't move
	LOAD	FailCode
	OR		FAIL_RIGHTMOTORNOSTART
	STORE	FailCode
	JUMP	RSTOPB
	
	
	
;Clear again
MotorPass:
	
	
	
AudioTest:
	
	

	

	CALL	ClearLCD
	LOAD	Zero
	ADDI	ListenStr_0
	STORE	StrStart
	CALL	StrToLCD

	
	
	CALL DITBLIT	;S
	CALL SYMSPACE
	CALL DITBLIT
	CALL SYMSPACE
	CALL DITBLIT
	CALL CHARSPACE
	
	CALL DAHBLIT	;O
	CALL SYMSPACE
	CALL DAHBLIT
	CALL SYMSPACE
	CALL DAHBLIT
	CALL CHARSPACE
	
	CALL DITBLIT	;S
	CALL SYMSPACE
	CALL DITBLIT
	CALL SYMSPACE
	CALL DITBLIT
	CALL CHARSPACE
	
	;; sonar test	
	
; vars for sonar test
CurrSonar:		DW		0
CurrEnSonar:		DW		1
CurrSonarButtonDown:	DW	0
CurrSonarButton2Down:	DW	0

SonarTest:	


	LOAD    Zero
	OUT     SONAREN     ; Disable sonar
	OUT     LVELCMD     ; Stop motors
	OUT     RVELCMD
	OUT		SEVENSEG

	OUT		TIMER		;timer to repeat until 2 sec is reached
	LOAD	One
	OUT     SEVENSEG    ; Output sum of wheel velocities to 7-seg
	
	LOAD	Zero		; init values
	STORE	CurrSonar
	LOAD	One
	STORE	CurrEnSonar
	
	; init LCD message
	LOAD	Zero
	ADDI	SonarInfoStr_0
	STORE	StrStart
	CALL	StrToLCD
	
	
	
	
MAINSON:		;FUNCTION TO DISPLAY SONAR DISTANCE
	OUT		TIMER	; reset timer
	LOAD	CurrEnSonar
	OUT		SONAREN		;Turn on the Sonar
	LOAD	CurrSonar
	ADDI	1			; show the sonar number on the seven seg
	OUT		SEVENSEG
	
	LOAD	Zero					; init state variaible of button
	STORE	CurrSonarButtonDown
	STORE	CurrSonarButton2Down
	
	;JZERO	SONFAIL		;if distance is <= 0, go to fail
	;JNEG	SONFAIL
	
MAINSON_TIMER_WAIT:
	CALL	SON_IN_DIST ; read in current distance
	STORE	HexValue
	LOAD	Zero
	ADDI	12
	STORE	HexValueLCDPos
	CALL	HexToLCD ; display current distance
	
	; handle key3 jumping
	IN		XIO
	AND		MaskKey3
	JZERO	SpinKeyJumpTest
	
	LOAD	CurrSonarButtonDown
	JZERO	MAINSON_TIMER_READ_BUTTON_UP
	JUMP	MAINSON_TIMER_READ_BUTTON_DOWN
MAINSON_TIMER_READ_BUTTON_UP:
	IN		XIO			; read key1 input bus
	XOR		AllOnes
	AND		MaskKey1	; mask off irrelevant bits
	JZERO	MAINSON_TIMER_READ_END ;if button is not down, just continue
	JUMP	MAINSON_TIMER_READ_BUTTON_UP_PRESS

MAINSON_TIMER_READ_BUTTON_UP_PRESS:
	LOAD	One
	STORE	CurrSonarButtonDown ; store "1" in the button down variable
	JUMP	MAINSON_TIMER_READ_END
MAINSON_TIMER_READ_BUTTON_DOWN:
	IN		XIO			; read key1 input bus
	XOR		AllOnes
	AND		MaskKey1	; mask off irrelevant bits
	; key came back up
	JZERO	MAINSON_INCREMENT
	JUMP	MAINSON_TIMER_READ_END
	
MAINSON_TIMER_READ_END:

; handle key2
LOAD	CurrSonarButton2Down
	JZERO	MAINSON_TIMER_READ_BUTTON_UP2
	JUMP	MAINSON_TIMER_READ_BUTTON_DOWN2
MAINSON_TIMER_READ_BUTTON_UP2:
	IN		XIO			; read key2 input bus
	XOR		AllOnes
	AND		MaskKey2	; mask off irrelevant bits
	JZERO	MAINSON_TIMER_READ_END2 ;if button is not down, just continue
	JUMP	MAINSON_TIMER_READ_BUTTON_UP_PRESS2

MAINSON_TIMER_READ_BUTTON_UP_PRESS2:
	LOAD	One
	STORE	CurrSonarButton2Down ; store "1" in the button down variable
	JUMP	MAINSON_TIMER_READ_END2
MAINSON_TIMER_READ_BUTTON_DOWN2:
	IN		XIO			; read key1 input bus
	XOR		AllOnes
	AND		MaskKey2	; mask off irrelevant bits
	; key came back up
	JZERO	MAINSON_DECREMENT
	JUMP	MAINSON_TIMER_READ_END2
	
MAINSON_TIMER_READ_END2:

	
	JUMP	MAINSON_TIMER_WAIT
MAINSON_INCREMENT:
	LOAD	CurrSonar
	ADDI	1
	STORE	CurrSonar
	ADDI	-8
	JZERO	EndSonar
	LOAD	CurrEnSonar
	SHIFT	1
	STORE	CurrEnSonar
	JUMP	MAINSON
	
MAINSON_DECREMENT:
	LOAD	CurrSonar
	JZERO	MAINSON ; dont allow negative sonar values
	ADDI	-1
	STORE	CurrSonar
	LOAD	CurrEnSonar
	SHIFT	-1
	STORE	CurrEnSonar
	JUMP	MAINSON
	

SON_IN_DIST:
	LOAD	CurrSonar	
	JZERO	SON_IN_DIST_0
	ADDI	-1
	JZERO	SON_IN_DIST_1
	ADDI	-1
	JZERO	SON_IN_DIST_2
	ADDI	-1
	JZERO	SON_IN_DIST_3
	ADDI	-1
	JZERO	SON_IN_DIST_4
	ADDI	-1
	JZERO	SON_IN_DIST_5
	ADDI	-1
	JZERO	SON_IN_DIST_6
	ADDI	-1
	JZERO	SON_IN_DIST_7
	
SON_IN_DIST_0:
	IN		DIST0
	RETURN
SON_IN_DIST_1:
	IN		DIST1
	RETURN
SON_IN_DIST_2:
	IN		DIST2
	RETURN
SON_IN_DIST_3:
	IN		DIST3
	RETURN
SON_IN_DIST_4:
	IN		DIST4
	RETURN
SON_IN_DIST_5:
	IN		DIST5
	RETURN
SON_IN_DIST_6:
	IN		DIST6
	RETURN
SON_IN_DIST_7:
	IN		DIST7
	RETURN

	

	
;; end sonar test
EndSonar:
	LOAD	Zero
	OUT		SONAREN
	JUMP	Pass



Wait1:
	OUT     TIMER		; One second pause subroutine
Wloop:
	IN      TIMER
	ADDI    -10
	JNEG    Wloop
	RETURN
	
	
TestAscii:	DW	&HABCD
	
; routine to show a hex value on the LCD
; hex value should be stored in HexValue
; desired char pos on LCD should be stored in HexValueLCDPos
HexValueLCDPos:		DW	1
HexValue:	DW	0

HexToLCD_LCD_Pos:	DW	0 ; local var to store lcd pos
HexDig3:	DW	0
HexDig2:	DW	0
HexDig1:	DW	0
HexDig0:	DW	0
HexMask:	DW	&H000F


HexTemp:	DW	0

HexToLCD:	
	; init vars
	LOAD	HexValueLCDPos
	STORE	HexToLCD_LCD_Pos
	LOAD	HexValue
	; get bits 15-12
	; shifting right is a "negative" shift
	SHIFT	-12	; A = A >> 12
	AND		HexMask ; mask off unneeded bits
	ADDI		C_0 ; A = addr of ascii digit
	STORE	HexTemp	; HexTemp = addr of ascii digit
	ILOAD	HexTemp	; A = ascii digit 3
	STORE	HexDig3	; HexDig3 = ascii digit 3
	
	
	LOAD	HexValue
	; get bits 11-8
	; shifting right is a "negative" shift
	SHIFT	-8	; A = A >> 8
	AND		HexMask ; mask off unneeded bits
	ADDI	C_0 ; A = addr of ascii digit
	STORE	HexTemp	; HexTemp = addr of ascii digit
	ILOAD	HexTemp	; A = ascii digit 2
	STORE	HexDig2	; HexDig2 = ascii digit 2
	
	LOAD	HexValue
	; get bits 7-4
	; shifting right is a "negative" shift
	SHIFT	-4	; A = A >> 4
	AND		HexMask ; mask off unneeded bits
	ADDI	C_0 ; A = addr of ascii digit
	STORE	HexTemp	; HexTemp = addr of ascii digit
	ILOAD	HexTemp	; A = ascii digit 1
	STORE	HexDig1	; HexDig1 = ascii digit 1
	
	LOAD	HexValue
	; get bits 3-0
	; shifting right is a "negative" shift
	SHIFT	0	; A = A >> 0
	AND		HexMask ; mask off unneeded bits
	ADDI	C_0 ; A = addr of ascii digit
	STORE	HexTemp	; HexTemp = addr of ascii digit
	ILOAD	HexTemp	; A = ascii digit 1
	STORE	HexDig0	; HexDig0 = ascii digit 1	
	
	; now output stuff to LCD
	LOAD	HexToLCD_LCD_Pos
	SHIFT	8		; shift up to the top 8 bits
	OR		HexDig3
	OUT		LCD
	; inc HexToLCD_LCD_Pos
	LOAD	HexToLCD_LCD_Pos
	ADDI	1
	STORE	HexToLCD_LCD_Pos

	SHIFT	8		; shift up to the top 8 bits
	OR		HexDig2
	OUT		LCD
	; inc HexToLCD_LCD_Pos
	LOAD	HexToLCD_LCD_Pos
	ADDI	1
	STORE	HexToLCD_LCD_Pos

	SHIFT	8		; shift up to the top 8 bits
	OR		HexDig1
	OUT		LCD
	; inc HexToLCD_LCD_Pos
	LOAD	HexToLCD_LCD_Pos
	ADDI	1
	
	SHIFT	8		; shift up to the top 8 bits
	OR		HexDig0
	OUT		LCD
	
	
	;LOAD	LCDPos4
	;OR		HexDig3
	;OUT		LCD
	;LOAD	LCDPos5
	;OR		HexDig2
	;OUT		LCD
	;LOAD	LCDPos6
	;OR		HexDig1
	;OUT		LCD
	;LOAD	LCDPos7
	;OR		HexDig0
	;OUT		LCD
	
	RETURN
	
; the "argument" to StrToLCD (address of start of string to be written)
StrStart:	DW	0

StrCurrMemPtr:		DW	0
StrCurrScreenPos:	DW	0
StrCurrScreenPosMask:	DW	0
StrLCDOut:	DW	0


StrToLCD:
	LOAD	Zero
	STORE	StrCurrScreenPos	; i = 0
	LOAD	Zero
	ADD		StrStart			
	STORE	StrCurrMemPtr		; ptr = strstart
StrToLCDLoop:
	; exit condition:
	ILOAD	StrCurrMemPtr
	ILOAD	StrCurrMemPtr	
	JZERO	StrToLCDLoopExit
	; calculate upper 8 bits(position)
	LOAD	StrCurrScreenPos
	SHIFT	8	; left shift 8 to get upper 8 bits of output word
	STORE	StrCurrScreenPosMask
	; load ascii char
	ILOAD	StrCurrMemPtr
	; OR with the correct position
	OR		StrCurrScreenPosMask
	STORE	StrLCDOut
	; output to LCD
	OUT		LCD
	; increment counter and pointer and continue
	LOAD	StrCurrScreenPos	
	ADDI	1
	STORE	StrCurrScreenPos
	LOAD	StrCurrMemPtr
	ADDI	1
	STORE	StrCurrMemPtr
	JUMP	StrToLCDLoop
StrToLCDLoopExit:
	RETURN
	
ClearLCD:
	LOAD	Zero
	ADDI	ClearStr_0
	STORE	StrStart
	CALL	StrToLCD
	RETURN
	
ShowLine2:
	LOAD	Zero
	ADDI	ChkIOStr_0 ; display line 2
	STORE	StrStart
	CALL	StrToLCD
	RETURN
		
SpinKeyTemp:	DW	0
	
; this debugging routing shows the accumulator on the sevenseg and waits for key1
SpinKeyShow:
	OUT		SEVENSEG
SpinKey:
	STORE	SpinKeyTemp
SpinKeyL1:
	; let K3 jump to an arbitrary test
	IN		XIO			; read pushbuttons
	AND		MaskKey3	; see if key3 is down
	JZERO	SpinKeyJumpTest ; if key3 is down jump to SpinKeyJumpTest
	
	IN		XIO			; read pushbuttons
	XOR		AllOnes		; A = not(A)
	AND		MaskKey1	; mask off irrelevant bits
	JZERO	SpinKeyL1	; spin until button is pushed
SpinKeyL2:
	IN		XIO			; read pushbuttons
	AND		MaskKey1	; mask off irrelevant bits
	JZERO	SpinKeyL2		; spin until button is released
	LOAD	SpinKeyTemp
	RETURN

SpinKeyJumpTest:
	; wait for k3 to be released
	IN		XIO
	AND		MaskKey3
	JZERO	SpinKeyJumpTest
	
	IN		SWITCHES
	STORE	TestTarget
	JUMP	JumpToTest
	
Fail:					; 
	LOAD	FailCode
	OR		FailDisplayMask
	OUT		SEVENSEG
	
	CALL	ShowLine2
	LOAD	Zero
	ADDI	FailInfoStr_0
	STORE	StrStart
	CALL	StrToLCD
	
	CALL	SpinKey
	CALL	ClearLCD
	LOAD	Zero
	OUT		SEVENSEG
	
	RETURN
	
CritFail:					; 
	LOAD	FailCode
	OR		FailDisplayMask
	OUT		SEVENSEG
	
	LOAD	Zero
	ADDI	CritFailInfoStr_0
	STORE	StrStart
	CALL	StrToLCD
	
	
	JUMP	CritFail
	
Pass:
	CALL	ClearLCD
	LOAD	Zero
	OUT		SEVENSEG
	ADDI	TestsCompleteStr_0
	STORE	StrStart
	CALL	StrToLCD
PassLoop:	JUMP	PassLoop
ShowLoop:	OUT		SEVENSEG
	JUMP	ShowLoop

; This is a good place to put variables
Temp:     			DW 0

; Useful variables for showing failure
FailCode:			DW 0
FAIL_INST_SET:		DW		1  ;  failure of the instruction set
FAIL_SONAR:			DW		2	; failure of sonar
FAIL_LEFTMOTORNOSTART:	DW		4 ; failure of left motor to read a reasonable value								(0000000000000100)
FAIL_RIGHTMOTORNOSTART:	DW		8 ; failure of right motor to read a reasonable value							(0000000000001000)
FAIL_LEFTMOTORVELTHRESH:  DW	16 ; failure of left velocity readings to fall within appropriate thresholds 	(0000000000010000)
FAIL_RIGHTMOTORVELTHRESH:  DW	32 ; failure of right velocity readings to fall within appropriate thresholds 	(0000000000100000)
FAIL_ADC:				DW	64 ; no battery reading
FAIL_LOWBATTERY:				DW	128 ; low battery reading
FailDisplayMask:	DW	&HF000


; Variables for the SCOMP test

ST_OPERAND_1_ADD_1:		DW	&H1235	
ST_OPERAND_2_ADD_1:		DW	&H1D9C			
ST_RESULT_ADD_1:		DW	&H2FD1

ST_OPERAND_1_ADD_2:		DW	&H39CB	
ST_OPERAND_2_ADD_2:		DW	&H091C
ST_RESULT_ADD_2:		DW	&H42E7

ST_OPERAND_1_SUB_1:		DW	&H43B4
ST_OPERAND_2_SUB_1:		DW	&HDEFA
ST_RESULT_SUB_1:		DW	&H64BA

ST_OPERAND_1_SUB_2:		DW	&HDDD0
ST_OPERAND_2_SUB_2:		DW	&HC477
ST_RESULT_SUB_2:		DW	&H1959

ST_OPERAND_1_AND_1:		DW	&H729D
ST_OPERAND_2_AND_1:		DW	&H14EE
ST_RESULT_AND_1:		DW	&H108C

ST_OPERAND_1_AND_2:		DW	&H9920
ST_OPERAND_2_AND_2:		DW	&H101F
ST_RESULT_AND_2:		DW	&H1000


ST_OPERAND_1_OR_1:		DW	&H159D
ST_OPERAND_2_OR_1:		DW	&H26BE
ST_RESULT_OR_1:		DW	&H37BF

ST_OPERAND_1_OR_2:		DW	&H4FC6
ST_OPERAND_2_OR_2:		DW	&H7E8A
ST_RESULT_OR_2:		DW	&H7FCE


ST_OPERAND_1_XOR_1:		DW	&H856F
ST_OPERAND_2_XOR_1:		DW	&HA156
ST_RESULT_XOR_1:		DW	&H2439

ST_OPERAND_1_XOR_2:		DW	&HA78F
ST_OPERAND_2_XOR_2:		DW	&H69CB
ST_RESULT_XOR_2:		DW	&HCE44

ST_OPERAND_1_SHIFT_RIGHT:	DW		&H394F
ST_RESULT_SHIFT_RIGHT:		DW	&H1CA7

ST_OPERAND_1_SHIFT_LEFT:	DW		&H1368
ST_RESULT_SHIFT_LEFT:		DW	&H26D0

ST_OPERAND_1_ADDI_1:		DW	&H6AA4 ;  adding the value &H0151
ST_RESULT_ADDI_1:		DW	&H6BF5

ST_OPERAND_1_ADDI_2:	DW		&H6AA4 ;  subtracting the value &H0083
ST_RESULT_ADDI_2:		DW	&H6A21
	
; motor test variables
; This is a good place to put variables
Speed:	  DW 0
RepeatSix: DW 6



; Having some constants can be very useful
Zero:     DW 0
One:      DW 1
;Two:      DW 2
;Three:    DW 3
;Four:     DW 4
;Five:     DW 5
;Six:      DW 6
;Seven:    DW 7
;Eight:    DW 8
;Nine:     DW 9
;Ten:      DW 10
Forward:  DW 100
Reverse:  DW -100
AllOnes:  DW &HFFFF
Eights:		DW	&H8888
PassCode: DW	&H9A55
;EnSonar0: DW &B00000001
;EnSonar1: DW &B00000010
;EnSonar2: DW &B00000100
;EnSonar3: DW &B00001000
;EnSonar4: DW &B00010000
;EnSonar5: DW &B00100000
;EnSonar6: DW &B01000000
;EnSonar7: DW &B10000000
EnSonars: DW &B11111111
MaskKey1: DW &B1		; mask for key 1
MaskKey2: DW &B10		; mask for key 2
MaskKey3: DW &B100		; mask for key 3

; ASCII CODES

C_0:	DW	48
C_1:	DW	49
C_2:	DW	50
C_3:	DW	51
C_4:	DW	52
C_5:	DW	53
C_6:	DW	54
C_7:	DW	55
C_8:	DW	56
C_9:	DW	57
C_A:	DW	65
C_B:	DW	66
C_C:	DW	67
C_D:	DW	68
C_E:	DW	69
C_F:	DW	70



CE_0:	EQU	48
CE_1:	EQU	49
CE_2:	EQU	50
CE_3:	EQU	51
CE_4:	EQU	52
CE_5:	EQU	53
CE_6:	EQU	54
CE_7:	EQU	55
CE_8:	EQU	56
CE_9:	EQU	57
CE_A:	EQU	65
CE_B:	EQU	66
CE_C:	EQU	67
CE_D:	EQU	68
CE_E:	EQU	69
CE_F:	EQU	70
CE_G:	EQU	71
CE_H:	EQU	72
CE_I:	EQU	73
CE_J:	EQU	74
CE_K:	EQU	75
CE_L:	EQU	76
CE_M:	EQU	77
CE_N:	EQU	78
CE_O:	EQU	79
CE_P:	EQU	80
CE_Q:	EQU	81
CE_R:	EQU	82
CE_S:	EQU	83
CE_T:	EQU	84
CE_U:	EQU	85
CE_V:	EQU	86
CE_W:	EQU	87
CE_X:	EQU	88
CE_Y:	EQU	89
CE_Z:	EQU	90
CE_SPACE:	EQU	32
CE_COLON:	EQU 58
CE_PERIOD:	EQU	46
CE_SLASH:	EQU	47



PreMotorStr_0:	DW	CE_T
PreMotorStr_1:	DW	CE_O
PreMotorStr_2:	DW	CE_G
PreMotorStr_3:	DW	CE_G
PreMotorStr_4:	DW	CE_L
PreMotorStr_5:	DW	CE_E
PreMotorStr_6:	DW	CE_SPACE
PreMotorStr_7:	DW	CE_S
PreMotorStr_8:	DW	CE_W
PreMotorStr_9:	DW	CE_1
PreMotorStr_10:	DW	CE_7
PreMotorStr_11:	DW	CE_SPACE
PreMotorStr_12:	DW	CE_T
PreMotorStr_13:	DW	CE_H
PreMotorStr_14:	DW	CE_E
PreMotorStr_15:	DW	CE_N
PreMotorStr_16:	DW	CE_K
PreMotorStr_17:	DW	CE_E
PreMotorStr_18:	DW	CE_Y
PreMotorStr_19:	DW	CE_1
PreMotorStr_20:	DW	CE_COLON
PreMotorStr_21:	DW	CE_SPACE
PreMotorStr_22:	DW	CE_C
PreMotorStr_23:	DW	CE_O
PreMotorStr_24:	DW	CE_N
PreMotorStr_25:	DW	CE_T
PreMotorStr_26:	DW	CE_I
PreMotorStr_27:	DW	CE_N
PreMotorStr_28:	DW	CE_U
PreMotorStr_29:	DW	CE_E
PreMotorStr_30:	DW	CE_SPACE
PreMotorStr_31:	DW	CE_SPACE
PreMotorStr_32:	DW	0

SonarInfoStr_0:	DW	CE_S
SonarInfoStr_1:	DW	CE_O
SonarInfoStr_2:	DW	CE_N
SonarInfoStr_3:	DW	CE_A
SonarInfoStr_4:	DW	CE_R
SonarInfoStr_5:	DW	CE_SPACE
SonarInfoStr_6:	DW	CE_D
SonarInfoStr_7:	DW	CE_I
SonarInfoStr_8:	DW	CE_S
SonarInfoStr_9:	DW	CE_T
SonarInfoStr_10:	DW	CE_COLON
SonarInfoStr_11:	DW	CE_SPACE
SonarInfoStr_12:	DW	CE_SPACE
SonarInfoStr_13:	DW	CE_SPACE
SonarInfoStr_14:	DW	CE_SPACE
SonarInfoStr_15:	DW	CE_SPACE
SonarInfoStr_16:	DW	CE_K
SonarInfoStr_17:	DW	CE_1
SonarInfoStr_18:	DW	CE_COLON
SonarInfoStr_19:	DW	CE_N
SonarInfoStr_20:	DW	CE_E
SonarInfoStr_21:	DW	CE_X
SonarInfoStr_22:	DW	CE_T
SonarInfoStr_23:	DW	CE_SPACE
SonarInfoStr_24:	DW	CE_K
SonarInfoStr_25:	DW	CE_2
SonarInfoStr_26:	DW	CE_COLON
SonarInfoStr_27:	DW	CE_P
SonarInfoStr_28:	DW	CE_R
SonarInfoStr_29:	DW	CE_E
SonarInfoStr_30:	DW	CE_V
SonarInfoStr_31:	DW	CE_SPACE
SonarInfoStr_32:	DW	0


FailInfoStr_0:	DW	CE_F
FailInfoStr_1:	DW	CE_A
FailInfoStr_2:	DW	CE_I
FailInfoStr_3:	DW	CE_L
FailInfoStr_4:	DW	CE_PERIOD
FailInfoStr_5:	DW	CE_SPACE
FailInfoStr_6:	DW	CE_S
FailInfoStr_7:	DW	CE_E
FailInfoStr_8:	DW	CE_E
FailInfoStr_9:	DW	CE_SPACE
FailInfoStr_10:	DW	CE_M
FailInfoStr_11:	DW	CE_A
FailInfoStr_12:	DW	CE_N
FailInfoStr_13:	DW	CE_U
FailInfoStr_14:	DW	CE_A
FailInfoStr_15:	DW	CE_L
FailInfoStr_32:	DW	0

CritFailInfoStr_0:	DW	CE_C
CritFailInfoStr_1:	DW	CE_R
CritFailInfoStr_2:	DW	CE_I
CritFailInfoStr_3:	DW	CE_T
CritFailInfoStr_4:	DW	CE_I
CritFailInfoStr_5:	DW	CE_C
CritFailInfoStr_6:	DW	CE_A
CritFailInfoStr_7:	DW	CE_L
CritFailInfoStr_8:	DW	CE_SPACE
CritFailInfoStr_9:	DW	CE_F
CritFailInfoStr_10:	DW	CE_A
CritFailInfoStr_11:	DW	CE_I
CritFailInfoStr_12:	DW	CE_L
CritFailInfoStr_13:	DW	CE_U
CritFailInfoStr_14:	DW	CE_R
CritFailInfoStr_15:	DW	CE_E
CritFailInfoStr_16:	DW	CE_S
CritFailInfoStr_17:	DW	CE_E
CritFailInfoStr_18:	DW	CE_E
CritFailInfoStr_19:	DW	CE_SPACE
CritFailInfoStr_20:	DW	CE_M
CritFailInfoStr_21:	DW	CE_A
CritFailInfoStr_22:	DW	CE_N
CritFailInfoStr_23:	DW	CE_U
CritFailInfoStr_24:	DW	CE_A
CritFailInfoStr_25:	DW	CE_L
CritFailInfoStr_32:	DW	0

ListenStr_0:	DW	CE_T
ListenStr_1:	DW	CE_E
ListenStr_2:	DW	CE_S
ListenStr_3:	DW	CE_T
ListenStr_4:	DW	CE_I
ListenStr_5:	DW	CE_N
ListenStr_6:	DW	CE_G
ListenStr_7:	DW	CE_SPACE
ListenStr_8:	DW	CE_B
ListenStr_9:	DW	CE_E
ListenStr_10:	DW	CE_E
ListenStr_11:	DW	CE_P
ListenStr_12:	DW	CE_S
ListenStr_13:	DW	CE_PERIOD
ListenStr_14:	DW	CE_PERIOD
ListenStr_15:	DW	CE_PERIOD
ListenStr_16:	DW	0


BattDoneStr_0:	DW	CE_B
BattDoneStr_1:	DW	CE_A
BattDoneStr_2:	DW	CE_T
BattDoneStr_3:	DW	CE_T
BattDoneStr_4:	DW	CE_E
BattDoneStr_5:	DW	CE_R
BattDoneStr_6:	DW	CE_Y
BattDoneStr_7:	DW	CE_SPACE
BattDoneStr_8:	DW	CE_O
BattDoneStr_9:	DW	CE_K
BattDoneStr_10:	DW	CE_SPACE
BattDoneStr_11:	DW	CE_SPACE
BattDoneStr_12:	DW	CE_SPACE
BattDoneStr_13:	DW	CE_SPACE
BattDoneStr_14:	DW	CE_SPACE
BattDoneStr_15:	DW	CE_SPACE
BattDoneStr_16:	DW	CE_K
BattDoneStr_17:	DW	CE_E
BattDoneStr_18:	DW	CE_Y
BattDoneStr_19:	DW	CE_1
BattDoneStr_20:	DW	CE_COLON
BattDoneStr_21:	DW	CE_SPACE
BattDoneStr_22:	DW	CE_C
BattDoneStr_23:	DW	CE_O
BattDoneStr_24:	DW	CE_N
BattDoneStr_25:	DW	CE_T
BattDoneStr_26:	DW	CE_I
BattDoneStr_27:	DW	CE_N
BattDoneStr_28:	DW	CE_U
BattDoneStr_29:	DW	CE_E
BattDoneStr_32:	DW	0

SCOMPPassStr_0:	DW	CE_S
SCOMPPassStr_1:	DW	CE_C
SCOMPPassStr_2:	DW	CE_O
SCOMPPassStr_3:	DW	CE_M
SCOMPPassStr_4:	DW	CE_P
SCOMPPassStr_5:	DW	CE_SPACE
SCOMPPassStr_6:	DW	CE_O
SCOMPPassStr_7:	DW	CE_K
SCOMPPassStr_8:	DW	CE_SPACE
SCOMPPassStr_9:	DW	CE_SPACE
SCOMPPassStr_10:	DW	CE_SPACE
SCOMPPassStr_11:	DW	CE_SPACE
SCOMPPassStr_12:	DW	CE_SPACE
SCOMPPassStr_13:	DW	CE_SPACE
SCOMPPassStr_14:	DW	CE_SPACE
SCOMPPassStr_15:	DW	CE_SPACE

SCOMPPassStr_32:	DW	0



ChkIOStr_0:	DW	CE_C
ChkIOStr_1:	DW	CE_H
ChkIOStr_2:	DW	CE_E
ChkIOStr_3:	DW	CE_C
ChkIOStr_4:	DW	CE_K
ChkIOStr_5:	DW	CE_SPACE
ChkIOStr_6:	DW	CE_O
ChkIOStr_7:	DW	CE_U
ChkIOStr_8:	DW	CE_T
ChkIOStr_9:	DW	CE_P
ChkIOStr_10:	DW	CE_U
ChkIOStr_11:	DW	CE_T
ChkIOStr_12:	DW	CE_SPACE
ChkIOStr_13:	DW	CE_SPACE
ChkIOStr_14:	DW	CE_SPACE
ChkIOStr_15:	DW	CE_SPACE
ChkIOStr_16:	DW	CE_K
ChkIOStr_17:	DW	CE_E
ChkIOStr_18:	DW	CE_Y
ChkIOStr_19:	DW	CE_1
ChkIOStr_20:	DW	CE_COLON
ChkIOStr_21:	DW	CE_SPACE
ChkIOStr_22:	DW	CE_C
ChkIOStr_23:	DW	CE_O
ChkIOStr_24:	DW	CE_N
ChkIOStr_25:	DW	CE_T
ChkIOStr_26:	DW	CE_I
ChkIOStr_27:	DW	CE_N
ChkIOStr_28:	DW	CE_U
ChkIOStr_29:	DW	CE_E
ChkIOStr_32:	DW	0

MotorWaitStr_0:	DW	CE_T
MotorWaitStr_1:	DW	CE_E
MotorWaitStr_2:	DW	CE_S
MotorWaitStr_3:	DW	CE_T
MotorWaitStr_4:	DW	CE_I
MotorWaitStr_5:	DW	CE_N
MotorWaitStr_6:	DW	CE_G
MotorWaitStr_7:	DW	CE_SPACE
MotorWaitStr_8:	DW	CE_M
MotorWaitStr_9:	DW	CE_T
MotorWaitStr_10:	DW	CE_R
MotorWaitStr_11:	DW	CE_S
MotorWaitStr_12:	DW	CE_PERIOD
MotorWaitStr_13:	DW	CE_PERIOD
MotorWaitStr_14:	DW	CE_PERIOD
MotorWaitStr_15:	DW	CE_SPACE
MotorWaitStr_16:	DW	0



; a big sequence of spaces
ClearStr_0:	DW	CE_SPACE
ClearStr_1:	DW	CE_SPACE
ClearStr_2:	DW	CE_SPACE
ClearStr_3:	DW	CE_SPACE
ClearStr_4:	DW	CE_SPACE
ClearStr_5:	DW	CE_SPACE
ClearStr_6:	DW	CE_SPACE
ClearStr_7:	DW	CE_SPACE
ClearStr_8:	DW	CE_SPACE
ClearStr_9:	DW	CE_SPACE
ClearStr_10:	DW	CE_SPACE
ClearStr_11:	DW	CE_SPACE
ClearStr_12:	DW	CE_SPACE
ClearStr_13:	DW	CE_SPACE
ClearStr_14:	DW	CE_SPACE
ClearStr_15:	DW	CE_SPACE
ClearStr_16:	DW	CE_SPACE
ClearStr_17:	DW	CE_SPACE
ClearStr_18:	DW	CE_SPACE
ClearStr_19:	DW	CE_SPACE
ClearStr_20:	DW	CE_SPACE
ClearStr_21:	DW	CE_SPACE
ClearStr_22:	DW	CE_SPACE
ClearStr_23:	DW	CE_SPACE
ClearStr_24:	DW	CE_SPACE
ClearStr_25:	DW	CE_SPACE
ClearStr_26:	DW	CE_SPACE
ClearStr_27:	DW	CE_SPACE
ClearStr_28:	DW	CE_SPACE
ClearStr_29:	DW	CE_SPACE
ClearStr_30:	DW	CE_SPACE
ClearStr_31:	DW	CE_SPACE
ClearStr_32:	DW	0

TestsCompleteStr_0:	DW	CE_T
TestsCompleteStr_1:	DW	CE_E
TestsCompleteStr_2:	DW	CE_S
TestsCompleteStr_3:	DW	CE_T
TestsCompleteStr_4:	DW	CE_I
TestsCompleteStr_5:	DW	CE_N
TestsCompleteStr_6:	DW	CE_G
TestsCompleteStr_7:	DW	CE_SPACE
TestsCompleteStr_8:	DW	CE_C
TestsCompleteStr_9:	DW	CE_O
TestsCompleteStr_10:	DW	CE_M
TestsCompleteStr_11:	DW	CE_P
TestsCompleteStr_12:	DW	CE_L
TestsCompleteStr_13:	DW	CE_E
TestsCompleteStr_14:	DW	CE_T
TestsCompleteStr_15:	DW	CE_E
TestsCompleteStr_16:	DW	CE_K
TestsCompleteStr_17:	DW	CE_E
TestsCompleteStr_18:	DW	CE_Y
TestsCompleteStr_19:	DW	CE_0
TestsCompleteStr_20:	DW	CE_COLON
TestsCompleteStr_21:	DW	CE_SPACE
TestsCompleteStr_22:	DW	CE_R
TestsCompleteStr_23:	DW	CE_E
TestsCompleteStr_24:	DW	CE_S
TestsCompleteStr_25:	DW	CE_T
TestsCompleteStr_26:	DW	CE_A
TestsCompleteStr_27:	DW	CE_R
TestsCompleteStr_28:	DW	CE_T
TestsCompleteStr_29:	DW	0

; IO address space map
SWITCHES: EQU &H00  ; slide switches
LEDS:     EQU &H01  ; red LEDs
TIMER:    EQU &H02  ; timer, usually running at 10 Hz
XIO:      EQU &H03  ; pushbuttons and some misc. I/0  DI & (Key 3..1)
SEVENSEG: EQU &H04  ; seven-segment display (4-digits only)
LCD:      EQU &H06  ; primitive 4-digit LCD display
LPOS:     EQU &H80  ; left wheel encoder position (read only)
LVEL:     EQU &H82  ; current left wheel velocity (read only)
LVELCMD:  EQU &H83  ; left wheel velocity command (write only)
RPOS:     EQU &H88  ; same values for right wheel...
RVEL:     EQU &H8A  ; ...
RVELCMD:  EQU &H8B  ; ...
I2C_CMD:  EQU &H90  ; I2C module's CMD register,
I2C_DATA: EQU &H91  ; ... DATA register,
I2C_BUSY: EQU &H92  ; ... and BUSY register
SONAR:    EQU &HA0  ; base address for more than 16 registers....
DIST0:    EQU &HA8  ; the eight sonar distance readings
DIST1:    EQU &HA9  ; ...
DIST2:    EQU &HAA  ; ...
DIST3:    EQU &HAB  ; ...
DIST4:    EQU &HAC  ; ...
DIST5:    EQU &HAD  ; ...
DIST6:    EQU &HAE  ; ...
DIST7:    EQU &HAF  ; ...
SONAREN:  EQU &HB2  ; register to control which sonars are enabled
XPOS:     EQU &HC0  ; Current X-position (read only)
YPOS:     EQU &HC1  ; Y-position
THETA:    EQU &HC2  ; Current rotational position of robot (0-701)
AUDIO_CTL:	  EQU &HC4  ; DAC audio signal enabler